function [dists, az, rev_az] =  vdist(lat1, lon1, lat2, lon2)
%   Calculates distances and bearings between points.
%
%   This uses Vincenty formula with an accuracy parameter used
%   to set the convergence of lambda. The default 1E-12 corresponds
%   to approximately 0.06 mm.
%
%   The formulas and nomenclature are from Vincenty, 1975:
%     https://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
%   See also:
%     https://en.wikipedia.org/wiki/Vincenty's_formulae
%
%   Inputs:
%     lat1, lon1: the initial point coodinates (in degrees) can be a vector
%     lat2, lon2: the final point coodinates (in degrees) can be a vector
%     accuracy: accuracy for the vincenty convergence (optional)
%
%   Returns:
%     s, A12, A21, distance (km), initial bearing (deg), and back bearing (deg).

% Input validation
if ~isnumeric(lat1) || ~isnumeric(lat2) || ~isnumeric(lon1) || ~isnumeric(lon2)
    error('All inputs must be numeric');
end

% Ensure scalar inputs are expanded to match vector inputs
if isscalar(lat1) && ~isscalar(lat2)
    lat1 = repmat(lat1, size(lat2));
end
if isscalar(lat2) && ~isscalar(lat1)
    lat2 = repmat(lat2, size(lat1));
end
if isscalar(lon1) && ~isscalar(lon2)
    lon1 = repmat(lon1, size(lon2));
end
if isscalar(lon2) && ~isscalar(lon1)
    lon2 = repmat(lon2, size(lon1));
end

% Check input sizes
if ~isequal(size(lat1), size(lat2), size(lon1), size(lon2))
    error('All inputs must have the same size or be scalar');
end

% Convert to column vectors for consistency
lat1 = lat1(:);
lat2 = lat2(:);
lon1 = lon1(:);
lon2 = lon2(:);

% Parameters
accuracy=1.0e-12;
a = 6378.1370;        % semi-major axis (km), WGS84
f = 1./298.257223563; % flattening of the ellipsoid, WGS84
b = (1-f)*a;          % semi-minor axis

% 输入为single，强转double用于内部计算
% phi1 = double(lat1);
% lambda1   = double(lon1);
% phi2 = double(lat2);
% lambda2    = double(lon2);
phi1 = lat1;
lambda1 = lon1;
phi2 = lat2;
lambda2 = lon2;

% Precompute reduced latitudes
tan_u1 = (1 - f) * tan(phi1);
tan_u2 = (1 - f) * tan(phi2);
cos_u1 = 1 ./ sqrt(1 + tan_u1.^2);
cos_u2 = 1 ./ sqrt(1 + tan_u2.^2);
sin_u1 = tan_u1 .* cos_u1;
sin_u2 = tan_u2 .* cos_u2;

% Longitude difference
L = lambda2 - lambda1;
lambda = L;

% Iterate until convergence
iter = 0;
max_iter = 100;
converged = false(size(lambda));

while iter < max_iter && any(~converged)
    sin_sigma = sqrt((cos_u2(~converged) .* sin(lambda(~converged))).^2 + ...
                     (cos_u1(~converged) .* sin_u2(~converged) - ...
                      sin_u1(~converged) .* cos_u2(~converged) .* cos(lambda(~converged))).^2);
    
    cos_sigma = sin_u1(~converged) .* sin_u2(~converged) + ...
                cos_u1(~converged) .* cos_u2(~converged) .* cos(lambda(~converged));
    
    sigma = atan2(sin_sigma, cos_sigma);
    
    sin_alpha = cos_u1(~converged) .* cos_u2(~converged) .* sin(lambda(~converged)) ./ sin_sigma;
    cos2_alpha = 1 - sin_alpha.^2;
    
    cos2_sigma_m = cos_sigma - 2 * sin_u1(~converged) .* sin_u2(~converged) ./ cos2_alpha;
    
    C = f/16 * cos2_alpha .* (4 + f * (4 - 3 * cos2_alpha));
    
    lambda_prev = lambda(~converged);
    lambda(~converged) = L(~converged) + (1 - C) * f .* sin_alpha .* ...
                         (sigma + C .* sin_sigma .* (cos2_sigma_m + C .* cos_sigma .* ...
                         (-1 + 2 * cos2_sigma_m.^2)));
    
    % Check convergence
    converged(~converged) = abs(lambda(~converged) - lambda_prev) <= accuracy;
    iter = iter + 1;
end

if iter == max_iter && any(~converged)
    warning('Vincenty formula did not converge for some points');
end

% Compute final values
sin_sigma = sqrt((cos_u2 .* sin(lambda)).^2 + ...
                 (cos_u1 .* sin_u2 - sin_u1 .* cos_u2 .* cos(lambda)).^2);
cos_sigma = sin_u1 .* sin_u2 + cos_u1 .* cos_u2 .* cos(lambda);
sigma = atan2(sin_sigma, cos_sigma);

sin_alpha = cos_u1 .* cos_u2 .* sin(lambda) ./ sin_sigma;
cos2_alpha = 1 - sin_alpha.^2;

cos2_sigma_m = cos_sigma - 2 * sin_u1 .* sin_u2 ./ cos2_alpha;

% Compute coefficients
u2 = cos2_alpha * (a^2 - b^2) / b^2;
A = 1 + u2/16384 * (4096 + u2 .* (-768 + u2 .* (320 - 175 * u2)));
B = u2/1024 * (256 + u2 .* (-128 + u2 .* (74 - 47 * u2)));

delta_sigma = B .* sin_sigma .* (cos2_sigma_m + B/4 .* (cos_sigma .* ...
              (-1 + 2 * cos2_sigma_m.^2) - B/6 .* cos2_sigma_m .* ...
              (-3 + 4 * sin_sigma.^2) .* (-3 + 4 * cos2_sigma_m.^2)));

% Compute distance in meters, then convert to km
dists = b * A .* (sigma - delta_sigma);

% Compute forward and reverse azimuths
az = atan2(cos_u2 .* sin(lambda), ...
           cos_u1 .* sin_u2 - sin_u1 .* cos_u2 .* cos(lambda));

rev_az = atan2(cos_u1 .* sin(lambda), ...
               -sin_u1 .* cos_u2 + cos_u1 .* sin_u2 .* cos(lambda));

% Normalize to [-pi, pi)
az = mod(az + pi, 2*pi) - pi;
rev_az = mod(rev_az + pi, 2*pi) - pi;

% Convert to single precision if needed
% dists = single(dists);
% az = single(az);
% rev_az = single(rev_az);

end
